#include "../include/alphadog_monitor/paramstuning_window.h"
#include "ui_paramstuning_window.h"
#include <QDoubleSpinBox>
#include <QCheckBox>
#include <QButtonGroup>
#include <QDateTime>

namespace alphadog_monitor {

paramstuning_window::paramstuning_window(QWidget *parent,QNode* _qnode) :
    QDialog(parent),
    ui(new Ui::paramstuning_window),
    qnode(_qnode)
{
    ui->setupUi(this);

    this->setWindowFlags(Qt::Dialog|Qt::FramelessWindowHint);

    // 初始化控件
    initWidget();

    // 更新参数
    updateCurveInf(&inf);
}

paramstuning_window::~paramstuning_window()
{
    delete ui;
}

void paramstuning_window::initWidget(void){
    // 最小化和关闭
    connect(ui->btnWinClose, &QPushButton::clicked, [=]() {
        saveParams();
        this->close();
    });
    connect(ui->btnWinMin, &QPushButton::clicked, [=]() { this->showMinimized(); });

    // 互斥checkBox
    QButtonGroup * group = new QButtonGroup(this);
    group->addButton(ui->checkBox_sin, 1);
    group->addButton(ui->checkbox_triangle, 2);
    group->addButton(ui->checkBox_quare, 3);

    // 配置保存
    connect(ui->Btn_SaveParams, &QPushButton::clicked, [=]() {
        updateCurveInf(&inf);
    });

    // 控制器设置
    controller_boxes.push_back(ui->SpinBox_Kp_X);
    controller_boxes.push_back(ui->SpinBox_Kd_X);
    controller_boxes.push_back(ui->SpinBox_Kp_Y);
    controller_boxes.push_back(ui->SpinBox_Kd_Y);
    controller_boxes.push_back(ui->SpinBox_Kp_Z);
    controller_boxes.push_back(ui->SpinBox_Kd_Z);
    controller_sliders.push_back(ui->Slider_Kp_X);
    controller_sliders.push_back(ui->Slider_Kd_X);
    controller_sliders.push_back(ui->Slider_Kp_Y);
    controller_sliders.push_back(ui->Slider_Kd_Y);
    controller_sliders.push_back(ui->Slider_Kp_Z);
    controller_sliders.push_back(ui->Slider_Kd_Z);
    for(uint32_t i = 0; i < controller_boxes.size(); i ++){
        connect(controller_boxes[i], &QDoubleSpinBox::editingFinished, [=]() {
            controller_sliders[i]->setValue(controller_boxes[i]->value());
        });
        connect(controller_sliders[i], &QSlider::sliderMoved, [=]() {
            controller_boxes[i]->setValue(controller_sliders[i]->value());
        });
    }

    // 创建定时器
    timer = new QTimer();
    connect(timer,SIGNAL(timeout()),this,SLOT(publishCmd()));
    connect(ui->Btn_enable, &QPushButton::clicked, [=]() { timer->start(1);});
    connect(ui->Btn_disable, &QPushButton::clicked, [=]() { timer->stop();});

    // 载入参数
    loadParams();
}

void paramstuning_window::publishCmd(void){
    // 计算输出
    calculateCurve(&inf, msgs);

    // 发布指令
    for(uint32_t i = 0; i < msgs.size(); i ++){
        qnode->FootCmdPuber.publish(msgs[i]);
    }
}

void paramstuning_window::updateCurveInf(Curve_Inf* _curve){
    _curve->freq[0] = ui->SpinBox_freq_X->value();
    _curve->freq[1] = ui->SpinBox_freq_Y->value();
    _curve->freq[2] = ui->SpinBox_freq_Z->value();

    _curve->amp[0] = ui->SpinBox_amp_X->value();
    _curve->amp[1] = ui->SpinBox_amp_Y->value();
    _curve->amp[2] = ui->SpinBox_amp_Z->value();

    _curve->origin[0] = ui->SpinBox_origin_X->value();
    _curve->origin[1] = ui->SpinBox_origin_Y->value();
    _curve->origin[2] = ui->SpinBox_origin_Z->value();

    if(ui->checkBox_sin->checkState()) _curve->curve_type = Curve_Sin;
    else if(ui->checkbox_triangle->checkState()) _curve->curve_type = Curve_Triangle;
    else if(ui->checkBox_quare->checkState()) _curve->curve_type = Curve_Quare;

    for(int i = 0; i < 3; i ++){
        _curve->Kp[i] = controller_boxes[2*i]->value();
        _curve->Kd[i] = controller_boxes[2*i + 1]->value();
    }

    _curve->axis_enable[0] = ui->checkBox_X->checkState();
    _curve->axis_enable[1] = ui->checkBox_Y->checkState();
    _curve->axis_enable[2] = ui->checkBox_Z->checkState();

    _curve->legs_enable[0] = ui->checkBox_RFleg->checkState();
    _curve->legs_enable[1] = ui->checkBox_LFleg->checkState();
    _curve->legs_enable[2] = ui->checkBox_RBleg->checkState();
    _curve->legs_enable[3] = ui->checkBox_LBleg->checkState();

    saveParams();
}

void paramstuning_window::calculateCurve(Curve_Inf* _curve, std::vector<sensor_msgs::JointState>& _msgs){
    float scale, pDes[3], vDes[3];
    int real_time = QDateTime::currentDateTime().toMSecsSinceEpoch();
    // 根据曲线类型确定输出
    for(int i = 0; i < 3; i ++){
        if(_curve->axis_enable[i]){
            float start = _curve->origin[i];
            float phase = (real_time % int(1. / _curve->freq[i] * 1000.)) / (1. / _curve->freq[i] * 1000.);
            float amp = _curve->amp[i];
            float freq = _curve->freq[i];
            switch (_curve->curve_type) {
            case Curve_Sin:
                scale = phase < 0.5f ? sin(M_PI * 2 * phase) : 0;
                vDes[i] = amp * cos(M_PI * 2 * phase);
                break;
            case Curve_Triangle:
                vDes[i] = (phase < 0.5f) ? amp * freq * 4.f : -amp * freq * 4.f;
                if(phase < 0.5f){
                    start -=  amp;
                    scale = 2 * vDes[i] * phase;
                }
                else{
                    start +=  0.5 * amp;
                    scale = 2 * vDes[i] * (phase - 0.5f);
                }

                break;
            case Curve_Quare:
                scale = phase > 0.5 ? -1. : 1.;
                vDes[i] = 0;
                break;
            default:
                break;
            }
            pDes[i] = start + scale * amp;
        }

    }


    // 幅值输出
    _msgs.clear();
    for(int leg = 0; leg < 4; leg ++){
        if(_curve->legs_enable[leg]){
            msg.position.push_back(_curve->axis_enable[0]? pDes[0] : _curve->origin[0]);
            msg.position.push_back(_curve->axis_enable[1]? pDes[1] : _curve->origin[1]);
            msg.position.push_back(_curve->axis_enable[2]? pDes[2] : _curve->origin[2]);
            for(int i = 0; i < 3; i ++) msg.position.push_back(_curve->Kp[i]);

            msg.velocity.push_back(_curve->axis_enable[0]? vDes[0] : 0);
            msg.velocity.push_back(_curve->axis_enable[1]? vDes[1] : 0);
            msg.velocity.push_back(_curve->axis_enable[2]? vDes[2] : 0);
            for(int i = 0; i < 3; i ++) msg.velocity.push_back(_curve->Kd[i]);

            msg.name.push_back(std::to_string(leg));
            _msgs.push_back(msg);
        }
    }
}

void paramstuning_window::saveParams(void){
    QSettings main_setting("alphadog_monitor","settings");
    main_setting.setValue("/params_tune/curve_type", inf.curve_type);

    main_setting.setValue("/params_tune/amp_x", ui->SpinBox_amp_X->value());
    main_setting.setValue("/params_tune/amp_y", ui->SpinBox_amp_Y->value());
    main_setting.setValue("/params_tune/amp_z", ui->SpinBox_amp_Z->value());

    main_setting.setValue("/params_tune/freq_x", ui->SpinBox_freq_X->value());
    main_setting.setValue("/params_tune/freq_y", ui->SpinBox_freq_Y->value());
    main_setting.setValue("/params_tune/freq_z", ui->SpinBox_freq_Z->value());

    main_setting.setValue("/params_tune/origin_x", ui->SpinBox_origin_X->value());
    main_setting.setValue("/params_tune/origin_y", ui->SpinBox_origin_Y->value());
    main_setting.setValue("/params_tune/origin_z", ui->SpinBox_origin_Z->value());

    main_setting.setValue("/params_tune/kp_x", ui->SpinBox_Kp_X->value());
    main_setting.setValue("/params_tune/kd_x", ui->SpinBox_Kd_X->value());
    main_setting.setValue("/params_tune/kp_y", ui->SpinBox_Kp_Y->value());
    main_setting.setValue("/params_tune/kd_y", ui->SpinBox_Kd_Y->value());
    main_setting.setValue("/params_tune/kp_z", ui->SpinBox_Kp_Z->value());
    main_setting.setValue("/params_tune/kd_z", ui->SpinBox_Kd_Z->value());

    main_setting.setValue("/params_tune/enable_x", ui->checkBox_X->checkState());
    main_setting.setValue("/params_tune/enable_y", ui->checkBox_Y->checkState());
    main_setting.setValue("/params_tune/enable_z", ui->checkBox_Z->checkState());

    main_setting.setValue("/params_tune/enable_RFleg", ui->checkBox_RFleg->checkState());
    main_setting.setValue("/params_tune/enable_LFleg", ui->checkBox_LFleg->checkState());
    main_setting.setValue("/params_tune/enable_RBleg", ui->checkBox_RBleg->checkState());
    main_setting.setValue("/params_tune/enable_LBleg", ui->checkBox_LBleg->checkState());
}

void paramstuning_window::loadParams(void){
    QSettings main_setting("alphadog_monitor","settings");
    Curve_Type type = Curve_Type(main_setting.value("/params_tune/curve_type").toInt());
    switch (type) {
    case Curve_Sin: ui->checkBox_sin->setChecked(true); break;
    case Curve_Quare: ui->checkBox_quare->setChecked(true); break;
    case Curve_Triangle: ui->checkbox_triangle->setChecked(true); break;
    default: break;
    }

    ui->SpinBox_amp_X->setValue(main_setting.value("/params_tune/amp_x").toDouble());
    ui->SpinBox_amp_Y->setValue(main_setting.value("/params_tune/amp_y").toDouble());
    ui->SpinBox_amp_Z->setValue(main_setting.value("/params_tune/amp_z").toDouble());

    ui->SpinBox_freq_X->setValue(main_setting.value("/params_tune/freq_x").toDouble());
    ui->SpinBox_freq_Y->setValue(main_setting.value("/params_tune/freq_y").toDouble());
    ui->SpinBox_freq_Z->setValue(main_setting.value("/params_tune/freq_z").toDouble());

    ui->SpinBox_origin_X->setValue(main_setting.value("/params_tune/origin_x").toDouble());
    ui->SpinBox_origin_Y->setValue(main_setting.value("/params_tune/origin_y").toDouble());
    ui->SpinBox_origin_Z->setValue(main_setting.value("/params_tune/origin_z").toDouble());

    ui->checkBox_X->setChecked(main_setting.value("/params_tune/enable_x").toBool());
    ui->checkBox_Y->setChecked(main_setting.value("/params_tune/enable_y").toBool());
    ui->checkBox_Z->setChecked(main_setting.value("/params_tune/enable_z").toBool());

    ui->checkBox_RFleg->setChecked(main_setting.value("/params_tune/enable_RFleg").toBool());
    ui->checkBox_LFleg->setChecked(main_setting.value("/params_tune/enable_LFleg").toBool());
    ui->checkBox_RBleg->setChecked(main_setting.value("/params_tune/enable_RBleg").toBool());
    ui->checkBox_LBleg->setChecked(main_setting.value("/params_tune/enable_LBleg").toBool());

    ui->SpinBox_Kp_X->setValue(main_setting.value("/params_tune/kp_x").toDouble());
    ui->SpinBox_Kd_X->setValue(main_setting.value("/params_tune/kd_x").toDouble());
    ui->SpinBox_Kp_Y->setValue(main_setting.value("/params_tune/kp_y").toDouble());
    ui->SpinBox_Kd_Y->setValue(main_setting.value("/params_tune/kd_y").toDouble());
    ui->SpinBox_Kp_Z->setValue(main_setting.value("/params_tune/kp_z").toDouble());
    ui->SpinBox_Kd_Z->setValue(main_setting.value("/params_tune/kd_z").toDouble());

    ui->Slider_Kp_X->setValue(main_setting.value("/params_tune/kp_x").toDouble());
    ui->Slider_Kd_X->setValue(main_setting.value("/params_tune/kd_x").toDouble());
    ui->Slider_Kp_Y->setValue(main_setting.value("/params_tune/kp_y").toDouble());
    ui->Slider_Kd_Y->setValue(main_setting.value("/params_tune/kd_y").toDouble());
    ui->Slider_Kp_Z->setValue(main_setting.value("/params_tune/kp_z").toDouble());
    ui->Slider_Kd_Z->setValue(main_setting.value("/params_tune/kd_z").toDouble());
}

void paramstuning_window::mousePressEvent(QMouseEvent *event)
{
    if (event->button() == Qt::LeftButton) {
        QPoint startPos = event->globalPos();
        m_offPos = startPos - geometry().topLeft();
    }
    QDialog::mousePressEvent(event);
}

void paramstuning_window::mouseMoveEvent(QMouseEvent *event)
{
    if (event->buttons() == Qt::LeftButton) {
        QPoint endPos = event->globalPos();
        move(endPos - m_offPos);
    }
    QDialog::mouseMoveEvent(event);
}

void paramstuning_window::mouseReleaseEvent(QMouseEvent *event)
{
    QDialog::mouseReleaseEvent(event);
}


}
